[M5Stack Core2 for AWS] サーボモーターで居眠りさせて見ました

SIN

2021.07.30

この記事は公開されてから1年以上経過しています。情報が古い可能性がありますので、ご注意ください。

1 はじめに

IoT事業部の平内（SIN）です。

前に、Arduinoを使用してM5Stack Core2 for AWSでサーボモーターを操作してみました。

今回、サーボモーターで、M5Stack Core2 for AWSの居眠りを表現してみました。最初に、動作確認している様子です。

2 カメラマウント

首の上下を表現するために使用したのは、SG90 Servo SG90サーボ用ミニ2軸オリジナル FPV 特殊なナイロン PTZ（NOサーボ）カメラマウント A838です。

こちらは、本来、サーボモーターを２つ組み込んで、2軸で利用するものなのですが、M5Stack Core2 for AWSをつけると、重すぎて動作が辛かったので、ちょっと改造して1軸で使用しました。

3 Face

顔を表現しているコードです。disp()で、4種類の顔を描画します。

face.h

#ifndef Face_h
#define Face_h

enum Expression {
    Normal,
    Surprised,
    Sleepy,
    Sleeping
};

class Face {
    public:
        Face();  
        void disp(Expression expression);
    private:
};

#endif

face.cpp

#include <M5Core2.h>
#include "Face.h"

Face::Face(){
    M5.Lcd.setBrightness(255);
    disp(Normal);
}

void Face::disp(Expression expression){
    M5.Lcd.fillScreen(WHITE);
    int center_y = M5.Lcd.height()/2;
    int center_x = M5.Lcd.width()/2;
    Serial.printf("Face::Expression %d\n", expression);
    if(expression == Normal) { // 大きい目
        M5.Lcd.fillCircle(center_x - 40, center_y - 30, 10, BLACK);
        M5.Lcd.fillCircle(center_x + 40, center_y - 30, 10, BLACK);
        M5.Lcd.fillRect(center_x - 15, center_y + 30, 30, 10, BLACK); 
    } else if(expression == Surprised) { // ちっちゃい目
        M5.Lcd.fillCircle(center_x - 40, center_y - 30, 5, BLACK);
        M5.Lcd.fillCircle(center_x + 40, center_y - 30, 5, BLACK);
        M5.Lcd.fillRect(center_x - 15, center_y + 30, 30, 10, BLACK); 
    } else if(expression == Sleepy) { // 半マブタ
        M5.Lcd.fillRect(center_x - 60, center_y - 35, 40, 5, BLACK); 
        M5.Lcd.fillRect(center_x + 20, center_y - 35, 40, 5, BLACK); 
        M5.Lcd.fillCircle(center_x - 40, center_y - 30, 5, BLACK);
        M5.Lcd.fillCircle(center_x + 40, center_y - 30, 5, BLACK);
        M5.Lcd.fillRect(center_x - 15, center_y + 30, 30, 5, BLACK); 
    } else if(expression == Sleeping) { // マブタ
        M5.Lcd.fillRect(center_x - 60, center_y - 35, 40, 5, BLACK); 
        M5.Lcd.fillRect(center_x + 20, center_y - 35, 40, 5, BLACK); 
        M5.Lcd.fillRect(center_x - 15, center_y + 30, 30, 5, BLACK); 
    }
}

4 Servo

首の上下を表現しているコードです。

move() で、変更する「首の角度」と、「スピード」を設定して、loop() を呼ぶことで、指定した状態に変化します。

servo.h

#ifndef Servo_h
#define Servo_h

class Servo {
    public:
        Servo();  
        void move(int angle, int speed);
        bool loop();
    private:
        int from_angle; // 変化前の角度
        int to_angle; // 変化後の角度
        int speed; // 変化のスピード
        bool moveing;
        void set_angle(int angle);
};

#endif

servo.cpp

#include <M5Core2.h>
#include "Servo.h"


#define SERVO_PIN  32
#define SERVO_CH   0
#define PWM_HZ 50
#define PWM_BIT 16 // 16bit(0～65535)

#define MAX 7800 // 65535 / 20 * 2.4 ≒ 7864 
#define MIN 1700 // 65535 / 20 * 0.5 ≒ 1638

Servo::Servo(){
    ledcSetup(SERVO_CH, PWM_HZ, PWM_BIT);
    ledcAttachPin(SERVO_PIN, SERVO_CH);

    from_angle = 0;
    to_angle = 0;
    speed = 0;
    moveing = true;
}

void Servo::move(int angle, int speed){
    Serial.printf("Servo::angle:%d speed:%d\n", angle, speed);
    this->speed = speed;
    to_angle = angle;
    moveing = true;
}

bool Servo::loop(){
    if(moveing){
        if(from_angle < to_angle){
            from_angle += 1;
        }else{
            from_angle -= 1;
        }
        set_angle(from_angle);
        delay(speed);
        if(to_angle == from_angle){
            moveing = false;
        }
    }
    return moveing;
}

5 main

メインのコードは、以下の通りです。Behavior構造体の配列で表現を逐次変更しています。

main.cpp

#include <M5Core2.h>
#include <stdio.h>
#include "Face.h"
#include "Servo.h"

Face *face;
Servo *servo;

struct Behavior{
    Expression expression;
    int angle;
    int speed;
};

struct Behavior behavior[]= {
    // dozing
    {Surprised, 90, 200},
    {Sleepy, 70, 50},
    {Sleeping, 90, 200},
    {Sleepy, 60, 100},
    {Sleeping, 90, 200},
    {Sleepy, 70, 50},
    {Sleeping, 90, 200},

    // get_up
    {Surprised, 20, 0},
    {Surprised, 20, 3000},
    {Normal, 30, 1000},
    {Surprised, 20, 500},
    {Normal, 20, 1000},
};

int n = 0;

void setup() {
    M5.begin(true, true, true, false); // I2C無効(PORTA をGPIOで使用するため)
    face = new Face();
    servo = new Servo();
}

void loop() {

    if(!servo->loop()){
        face->disp(behavior[n].expression);
        servo->move(behavior[n].angle, behavior[n].speed);

        n++;
        if(sizeof(behavior)/sizeof(behavior[0]) <= n){
            n = 0;
        }
        Serial.printf("n:%d\n", n);
    }
}